

#include "set_parameter.h"
#include "HeaderFiles.h"


STR_PID_HDL linespeedHdl[2];
STR_PID_HDL angularspeedHdl;

TRAJ_Obj freqReftraj[2];


STR_MOTOR_PARAM  wheelMotor_x1 =  {
                                       .PolePairs = 5,
                                       .Rs_ohm    = 0.075f,
                                       .Ld_H      = 0.000155f,
                                       .Lq_H      = 0.000155f,
                                       .Flux_VS   = 0.07815f
                                  };

								  
STR_MOTOR_PARAM wheelMotor_10530 = {
                                       .PolePairs = 15,
                                       .Rs_ohm    = 0.15f,
                                       .Ld_H      = 0.0005f,
                                       .Lq_H      = 0.0005f,
                                       .Flux_VS   = 0.07815f
                                  };


void parameter_init(void)
{
	
	pid_handle_init(&angularspeedHdl);

	for(uint16_t i=0;i<2;i++)
	{
	
		pid_handle_init(&linespeedHdl[i]);
		
		
		// set linespeed control kp ki value
		linespeedHdl[i].kp = 0.2;
		linespeedHdl[i].ki = 0.02;

		
		freqReftraj[i].intValue    = 0;
		freqReftraj[i].targetValue = 0;
		freqReftraj[i].maxValue    = REF_SPEED_MAX_HZ;
		freqReftraj[i].minValue    = -REF_SPEED_MAX_HZ;
		freqReftraj[i].maxDelta    = REF_SPEED_ACC_MAX_HZ_S;

	}	
	
	motorctrl_init();
	
	param_diag_init();
	
	//dataLedSet.real.state = (1 << POWER_ON);
	
	userHdl.signal.bumper_old[0] = 1;
	userHdl.signal.bumper_old[1] = 1;
	
	userHdl.ctrl.ctrlmode = SYS_CTRL_MODE;
}


